#pragma once
#include "DistanceField.h"
#include "AttachCollDetection.h"
#include "Parameters.h"
#include <cmath>
#include <algorithm>
#include <cfloat>
#include <iostream>
#include <vector>
#include <algorithm>
#include "time.h"
#include<omp.h>

using namespace std;

/* Voxel size is the length of half edge */

#define SQRT2 1.42
#define OMP_CPU 8
#define COARSE_BVH_YES

int DistanceField::_subDivisionCount = 0;

floatVec3* floatVec1TofloatVec3(float *vec1, floatVec3 *vec3, int size) {
	size = size;
	for(int i = 0; i < size; i++) {
		vec3[i][0] = vec1[i * 3];
		vec3[i][1] = vec1[i * 3 + 1];
		vec3[i][2] = vec1[i * 3 + 2];
	}
	return vec3;
}

float eucdist(float x, float y, float z, float x1, float y1, float z1) {	
	return sqrt( (x-x1)*(x-x1) + (y-y1)*(y-y1) + (z-z1)*(z-z1));
}

DistanceField::DistanceField(void)
{
}

DistanceField::DistanceField(int nSubVolume, int nSubMesh, MeshStruct* src, MeshStruct* trg)
{
	_src = src;
	_trg = trg;
	_nSubVolume = nSubVolume;	
	_nSubMesh = nSubMesh;
	_fineGrainOctree = _triSoupOctree = 0;
}

void DistanceField::destroy(DistanceVoxel *node) {
	/*
	for(int i = 0; i < 8; i++) {
		if(node->m_sibling[i]) {
			destroy(node->m_sibling[i]);
		}
	}
	delete node;
	*/
}

DistanceField::~DistanceField(void)
{
	//DistanceField::destroy(_fineGrainOctree);
	delete _fineGrainOctree;
	delete _triSoupOctree;
}

float min(float v1,float v2)
{
	if (v1>v2)
		return v2;
	return v1;
}

float max(float v1,float v2)
{
	if (v1<v2)
		return v2;
	return v1;
}

BoundingVolume DistanceField::getBoundingVolume() {\
	int n = _src->m_nGeom;
	float x, X, y, Y, z, Z;
	x = y = z = FLT_MAX;
	X = Y = Z = FLT_MIN;
	for(int i = 0; i < n; i=i+3) {
		x = min(x, _src->m_geom[i]);
		X = max(X, _src->m_geom[i]);
		y = min(y, _src->m_geom[i+1]);
		Y = max(Y, _src->m_geom[i+1]);
		z = min(z, _src->m_geom[i+2]);
		Z = max(Z, _src->m_geom[i+2]);
	}	
	float size = ( max( Z, max(X, Y) ) - min( y, min(x, z) ) ) / 2;
	std::cout << "Bounding Volume Size: " << size << std::endl;
	std::cout << "Bounding Volume Center: (" << (x + X)/2 <<" "<< (y+Y)/2<<" "<< (z+Z)/2 << ")" << std::endl;
	return BoundingVolume(size, (x + X)/2, (y+Y)/2, (z+Z)/2);
}


// METOHDS important

void DistanceField::init() {
	_nV = 0;
	// Calculate bounding volume
	_bv = getBoundingVolume();

#ifdef COARSE_BVH_YES
	// Init Coarse grained
	_subDivisionCount = 0;	
	initTriSoupOctree();
#endif

	// Init fine grained
	_subDivisionCount = 0;
	double start = clock();
	initFineGrainOctree();
	double finish = clock();
	std::cout << "FINE Elapsed time: " << (finish - start)/CLOCKS_PER_SEC <<"sec" << std::endl;
}

void DistanceField::initTriSoupOctree() {
	float emptyC[3] = {0.0f, 0.0f, 0.0f};
	float* center = _bv.getCenter(emptyC);
	float size = _bv.getSize();
	m_CoarseVoxelSize = size;
	_triSoupOctree = new TriangleSoupVoxel(size, 0, 0, 0, center[0], center[1], center[2]);
	center = 0;
	split(_triSoupOctree);
}

void DistanceField::initFineGrainOctree() {
	float emptyC[3] = {0.0f, 0.0f, 0.0f};
	float* center = _bv.getCenter(emptyC);
	float size = _bv.getSize();
	m_voxelSize = size;
	_fineGrainOctree = new DistanceVoxel(size, 0, center[0], center[1], center[2]);
	
#ifdef OMP_CPU
	int iCPU = omp_get_num_procs();
	std::cout << "the number of processors in this system: " << iCPU << std::endl;	
	// Now set the number of threads
	if(OMP_CPU <= iCPU) {
		omp_set_num_threads(OMP_CPU);
	} else {
		omp_set_num_threads(iCPU);
	}	
#endif

	float newSize = _fineGrainOctree->m_size / 2;	
	int nodeid = 0;
	int depth = _fineGrainOctree->getDepth() + 1;
	
	int signX[] = {1, 1, 1, 1, -1, -1, -1, -1};
	int signY[] = {1,1,-1,-1, 1, 1, -1, -1};
	int signZ[] = {1,-1,1,-1,1,-1,1,-1};

#ifdef OMP_CPU
#pragma omp parallel for 
#endif
	for(int ithread = 0; ithread < 8; ithread++) {
		float newX = _fineGrainOctree->m_x + signX[ithread] * newSize;
		float newY = _fineGrainOctree->m_y + signY[ithread] * newSize;									
		float newZ = _fineGrainOctree->m_z + signZ[ithread] * newSize;

		int depth = _fineGrainOctree->getDepth() + 1;
		_fineGrainOctree->m_sibling[nodeid] = new DistanceVoxel(newSize, depth, newX, newY, newZ);
		split(_fineGrainOctree->m_sibling[nodeid++]);			
	}	
}

// (pre-computation stage, off-line)Compute shortest distance from (x, y, z) to the target mesh 
float DistanceField::computeFromPoint(float x, float y, float z) {	
	// Attach Collision Detection
	AttachCollDetection	*collDetect_ = new AttachCollDetection();	

	// Create first object made of ONE single triangle
	int* pointTri = new int[5];
	for(int j = 0; j < 3; j++) 
	{
		pointTri[j] = j;
	}
	pointTri[3] = -1;
	pointTri[4] = -1;

	floatVec3 *pointGeom = new floatVec3[3];
	//float pointGeomVec1[9] = {x, y+0.0001, z, x+0.0001, y, z, x, y, z + 0.0001};
	float pointGeomVec1[9] = {x, y, z + m_voxelSize,  x+m_voxelSize, y, z, x, y+m_voxelSize, z};
	pointGeom = floatVec1TofloatVec3(pointGeomVec1, pointGeom, 3);
	collDetect_->SetObj2Mesh(pointTri, 1, pointGeom, 3);
	//collDetect_->SetObj1Mesh(pointTri, 1, pointGeom, 3);
	
	int trgNumTri = 0;
	int trgNumV = 0;
	int *tri_vec1 = NULL;
	float *geom_vec1 = NULL;

#ifdef COARSE_BVH_YES
	// Create second object from closest polygon soup in BVH
	TriangleSoupVoxel* tsv = coarseVoxelClosestTo(x, y, z);
	trgNumTri = tsv->getTriNum();
	trgNumV = tsv->getVertNum();
	tri_vec1 = tsv->getTris();
	geom_vec1 = tsv->getVerts();
#else
	trgNumTri = _trg->m_nTri/3;
	trgNumV = _trg->m_nGeom/3;
	tri_vec1 = _trg->m_tri;
	geom_vec1 = _trg->m_geom;
#endif		

	floatVec3 * geom2_vec3 = new floatVec3[trgNumV];
	geom2_vec3 = floatVec1TofloatVec3(geom_vec1, geom2_vec3, trgNumV);

	int* faces = new int[4*trgNumTri + 1];
	int tri = 0;
	for(tri = 0; tri < trgNumTri; tri++) 
	{
		for(int ii = 0; ii < 3; ii++)
		{
			faces[tri*4 + ii] = tri_vec1[tri*3 + ii] - 1;
		}
		faces[tri*4 + 3] = -1;
	}
	faces[tri*4] = -1;

	collDetect_->SetObj1Mesh(faces, trgNumTri, geom2_vec3, trgNumV);

	collDetect_->ComputeCollision(_bv.getSize());	

	float collDist = 0.0f;
	
	int goodCollNum = 0;
	for(int i = 0; i < 3; i++) {
		//float size  = _bv.getSize();
		if(collDetect_->collInfo[i].collDist < _bv.getSize()) {
			collDist += collDetect_->collInfo[i].collDist;
			goodCollNum++;
		}
	}
	if(goodCollNum>0) {
		collDist /= goodCollNum;
	} else {
		collDist = collDetect_->collInfo[0].collDist;
	}
	
	//cout << collDist << endl;

	delete collDetect_;
	delete[] faces;
	delete[] pointTri;
	delete[] pointGeom;
	delete[] geom2_vec3;
	
	return collDist;
}

// Split FINE octree node into 8 octants
void DistanceField::split(DistanceVoxel* node) {
	if(node->getDepth() > _nSubVolume) {
		return;
	} else if(node->getDepth() == _nSubVolume) {
		// Calculate distance
		node->setDistance(computeFromPoint(node->m_x, node->m_y, node->m_z));
		m_voxelSize = node->m_size;
		//std::cout<<"NewVoxel"<<std::endl;
		_nV++;
		return;
	} else {
		float newSize = node->m_size / 2;	
		int nodeid = 0;
		for(int ix = -1; ix <=1; ix=ix+2) {
			float newX = node->m_x + ix * newSize;
			for(int iy = -1; iy <= 1; iy=iy+2) {
				float newY = node->m_y + iy * newSize;
					for(int iz = -1; iz <= 1; iz=iz+2) {
						float newZ = node->m_z + iz * newSize;						
						// Calculate distance
						//float newDistance = computeFromPoint(newX, newY, newZ);
						//node->m_distance = newDistance;
						// Create new sub-node
						int depth = node->getDepth() + 1;
						node->m_sibling[nodeid] = new DistanceVoxel(newSize, depth, newX, newY, newZ);
						//node->m_sibling[nodeid] = new DistanceVoxel(newSize, newDistance, depth, newX, newY, newZ);					
						split(node->m_sibling[nodeid++]);
					}					
			}
		}
	}
}

// Split COARSE octree node into 8 octants
void DistanceField::split(TriangleSoupVoxel* node) {
	int i;
	// If more than maximum depth reached, then stop
	if(node->getDepth() > _nSubMesh) {
		return;
	} else
		// If maximum depth:
		if(node->getDepth() == _nSubMesh) {					
			// Init triangle soup inside the volume of current voxel
			std::vector<float>	verticesVector;		// vector of vertex coordinates
			std::vector<int>	verticesIdVector;	// vector of vertex indices which are within voxel volume
			std::vector<int>	trianglesVector;	// vector of triangles (in the form of vertex indices)

			m_CoarseVoxelSize = node->m_size;
			// Iterate over vertices
			int nGeom = _trg->m_nGeom/3;
			for(i = 0; i < nGeom; i++) {
				// Check if next vertex is inside the volume of the voxel
				float vertex2voxelDist = eucdist( node->m_x, node->m_y, node->m_z, _trg->m_geom[i*3], _trg->m_geom[i*3 + 1], _trg->m_geom[i*3 + 2] );
				if(vertex2voxelDist <= SQRT2 * m_CoarseVoxelSize) {	
					verticesIdVector.push_back(i);
					//std::cout << "Push back vertex: " << i << std::endl;
					//std::cout << "( ";
					for(int iv = 0; iv < 3; iv++) {
						float newVertex = _trg->m_geom[i*3 + iv];
						verticesVector.push_back( (newVertex + 1));
						//std::cout << newVertex << " ";
					}
					//std::cout << ")" << std::endl;
			}	
			}			

			// Iterate over triangles
			int nTri = _trg->m_nTri / 3;
			for(i = 0; i < nTri; i++) {
				bool triIsInsideVoxel = true;
				for(int it = 0; it < 3; it++) {
					int triVertId = _trg->m_tri[i*3 + it] - 1;
					if(std::find(verticesIdVector.begin(), verticesIdVector.end(), triVertId) == verticesIdVector.end()) {
						triIsInsideVoxel = false;
					}
				}
				if(triIsInsideVoxel) {
					//std::cout << "< ";
					for(int it = 0; it < 3; it++) {
						int triVertId = _trg->m_tri[i*3 + it];
						trianglesVector.push_back(triVertId);
						//std::cout << triVertId << " ";
					}
					//std::cout << ">" << std::endl;
				}
			}

			// If the node does not enclose any geometry mark it as stillborn and remove from parent
			if(trianglesVector.size() == 0) {
				(node->getParent())->setStillBorn(node);
			} else {
				// Otherwise, set triangle soup to the voxel
				(node)->setEnclosedTriangleSoup(verticesVector, trianglesVector);
			}

			return;
		} // (end) if maximum depth

		// If less than maximum depth
		else {
			float newSize = node->m_size / 2;	
			int nodeid = 0;
			for(int ix = -1; ix <=1; ix=ix+2) {
				float newX = node->m_x + ix * newSize;
				for(int iy = -1; iy <= 1; iy=iy+2) {
					float newY = node->m_y + iy * newSize;
					for(int iz = -1; iz <= 1; iz=iz+2) {
						// Create new octant
						float newZ = node->m_z + iz * newSize;												
						int depth = node->getDepth() + 1;	// update depth								
						node->increaseNumChildren();
						node->_sibling[nodeid] = new TriangleSoupVoxel(newSize, depth, nodeid, node, newX, newY, newZ);						
						split(node->_sibling[nodeid++]);
					}					
				}
			}
		}
}

// (post-computation, on-line) Given precomputed BVHs, get shortest distance to the target from (x, y , z)
float DistanceField::distanceFromPoint(DistanceVoxel *node, float x, float y, float z) {	
	if(node->m_sibling[0] == 0) {
		return eucdist(node->m_x, node->m_y, node->m_z, x, y, z);
	} else {
		int closestVoxelId = 0;
		float minDist = FLT_MAX;
		for(int i = 0; i < 8; i++) {
			float x_ = node->m_sibling[i]->m_x;
			float y_ = node->m_sibling[i]->m_y;
			float z_ = node->m_sibling[i]->m_z;
			float curDist = eucdist(x_, y_, z_, x, y, z);
			if( curDist < minDist) {
				minDist = curDist;
				closestVoxelId = i;
			}
		}
		return DistanceField::distanceFromPoint(node->m_sibling[closestVoxelId], x, y, z);
	}
}

// (post-computation, on-line) Get sum of distances from input 'mesh' to the target mesh
// @TODO
float* DistanceField::distanceFromMesh(MeshStruct mesh) {
	return 0;
}

TriangleSoupVoxel* DistanceField::closestTo(TriangleSoupVoxel* root, float x, float y, float z) {
	int closestVoxelId = -1;
	float minDist = FLT_MAX;
	for(int i = 0; i < 8; i++) {
		TriangleSoupVoxel *node = root->_sibling[i];
		if(node) {
			float x_ = node->m_x;
			float y_ = node->m_y;
			float z_ = node->m_z;
			float curDist = eucdist(x_, y_, z_, x, y, z);
			if( curDist < minDist) {
				minDist = curDist;
				closestVoxelId = i;
			}
		}
	}
	if(closestVoxelId > -1 ) {
		return closestTo(root->_sibling[closestVoxelId], x, y, z);	
	} else {
		return root;
	}
}
/*
DistanceVoxel* DistanceField::fineVoxelClosestTo(float x, float y, float z) {
	return closestTo(_fineGrainOctree, x, y, z);
}
*/

TriangleSoupVoxel*	DistanceField::coarseVoxelClosestTo(float x, float y, float z) {
	return closestTo(_triSoupOctree, x, y, z);
}
